/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * Drives J2B2 with a joystick and keyboard
 * TODO:
 - parameters
 - Gimbotus
 - Documentation
*/

// SDL includes
#include <SDL/SDL.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "ownutils.h"
#include "SpeedCtrlClient.hpp"
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//*****************************************************************************
// Objects

static gimi::GIMI g;
static MaCI::SpeedCtrl::CSpeedCtrlClient scc(&g, 0);

// Speed limit value +/-
static float v_lim = 0.5;

// Angular speed limit value +/-
static float w_lim = 3.14159;

// Current speed
static float v = 0.0;

// Current angular speed
static float w = 0.0;

// Current acceleration
static float a = 0.2;
static float a_lim = 0.2;


//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
static volatile bool run = true;
static ownThreadHandle  speedSendingThread;
static bool setSpeedCommanded = false;
//*****************************************************************************

static std::string macigroup = "J2B2";
static std::string groupinstance = "Motion";

//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tSpeedCtrlClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-s name]\t\t\tUse 'name' as SpeedCtrl target\n");
  fprintf(stderr,"  [-K key]\t\t\tUse 'key' as access key for SpeedCtrl interface\n");
  fprintf(stderr,"  [-G name]\t\t\tSpecify the MaCIGroup for SpeedCtrl target\n");
  fprintf(stderr,"  [-I name]\t\t\tSpecify the MaCIGroup Interface Instance\n");
  

  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
static int SpeedSendingThread(void *)
{
  unsigned int sleepTime = 50;
  ownTime_ms_t timeoutTime;
  if(!scc.GetSpeedCtrlServerTimeoutTime(timeoutTime,1000)){
    return 0;
  }

  if(timeoutTime < sleepTime){
    sleepTime = timeoutTime/2;
  }

  while(run){
    if(setSpeedCommanded){
      // Cut the speed to limits
      if (v > v_lim) v = v_lim;
      else if (v < -v_lim) v = -v_lim;
      if (w > w_lim) w = w_lim;
      else if (w < -w_lim) w = -w_lim;
      
      scc.SetSpeed(v,w,a);
    }
    ownSleep_ms(sleepTime);
  }
  return 0;

}

int main(int argc, char *argv[])
{
  using namespace MaCI;
  // GIMnet parameters
  std::string gimnetAP = "192.168.10.1";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string gimnetSourceName = "";
  
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(2);
  debugSetLogFilename("SpeedCtrlClientExample.log");

  // Key.
  unsigned char akey[16];
  memset(akey,0x00,sizeof(akey));
  
  // Greet
  fprintf(stderr,
          "\nSpeedCtrlClientExample - MaCI SpeedCtrl Interface Example - v0.0\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "s:n:u:p:G:I:K:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

    case 'K':
      strncpy((char*)akey, optarg, sizeof(akey));
      dPrint(3,"Using MaCI Key: '%s'", optarg);
      break;

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 's':
      gimnetSourceName = optarg;
      dPrint(3,"Using GIMnet source name '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

    case 'G':
      macigroup = optarg;
      dPrint(3,"Using MaCI-Group '%s'", optarg);
      break;

    case 'I':
      groupinstance = optarg;
      dPrint(3,"Using MaCI-Group Interface Instance '%s'", optarg);
      break;


      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
        debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  MaCI::EMaCIError e;
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
           gimnetAP.c_str(), 
           gimnetAPPort, 
           gimnetName.size()?gimnetName.c_str():"<anonymous>",
           gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
           gimnetAP.c_str(), 
           gimnetAPPort, 
           gimnetName.size()?gimnetName.c_str():"<anonymous>",
           gimi::getGimiErrorString(r).c_str());


    // Set target
    if (macigroup.size() && groupinstance.size()){
  
      std::string maciname;
      //sprintf(maciname, "%s.MaCI_SpeedCtrl.%s", macigroup, groupinstance);
      maciname = macigroup + ".MaCI_SpeedCtrl." + groupinstance;
      scc.SetDefaultTarget(MaCI::MaCICtrl::SMaCISL(maciname), 4000);
    }
    else{ 
  
      scc.SetDefaultTarget(gimnetSourceName);
    }
    //   scc.SetClientInterfaceAuthentication(MaCI::MaCICtrl::TAuthentication(akey, sizeof(akey)));

    // Attempt to Open Client
    if ( (e = scc.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open SpeedCtrlClient instance! (%s)",
             GetErrorStr(e).c_str());
      
    } else {

      //THIS IS FOR TESTING
      MaCI::SpeedCtrl::TSpeed max;
      MaCI::SpeedCtrl::TSpeed min;

      if(scc.GetSpeedLimits(max,min)){
        v_lim = max.speed;
        w_lim = max.angularSpeed;
        a_lim = max.acceleration;
        dPrint(3,"SpeedLimits max %f %f %f, min %f %f %f", max.speed,max.angularSpeed,max.acceleration,
               min.speed,min.angularSpeed,min.acceleration);
      }else{
        dPrint(3,"Get speed limits returned false...");
      }		
    }
    // Start signal handler thread
    speedSendingThread = ownThread_Create((void*)SpeedSendingThread, NULL);
  }
		
		
		
		
		
		
		
		
  /************************************************************************************
   */
  SDL_Surface *screen = NULL;
	SDL_Joystick *joystick;     //< The SDL Joystick pointer
	
  /////////////////////////////////////////////////////////////////////////////
  // Initialize SDL
  if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) == -1){
    fprintf(stderr,"Failed to initialize SDL (Video & Joystick): %s\n", SDL_GetError());
    exit(1);
  }

  // Register cleanup function
  atexit(SDL_Quit);

  // Initialize SDL-Video in mode 800x600, 32bpp, & some flags.
  if ((screen = SDL_SetVideoMode(100, 100, 32, 
                                 SDL_HWSURFACE|SDL_DOUBLEBUF|SDL_RESIZABLE)) == NULL) {
    fprintf(stderr, "Unable to set video mode for SDL Context: %s\n", SDL_GetError());
    exit(1);
  }  
  
  // Clear the screen
  SDL_FillRect(screen, NULL, 0);
	
	SDL_JoystickEventState(SDL_ENABLE);
	joystick = SDL_JoystickOpen(0);
	
  /////////////////////////////////////////////////////////////////////////////
  // Simple event processing example
  SDL_Event event;
  bool run = true;
	float maxSpeed=.2;
	float lSpeed = 0.0;
	float rSpeed = 0.0;
	float speedDir = 1.0;
	bool autopilot = false;
  fprintf(stderr,"Entering SDL Event Processing loop...");
  while (run) {
    // Wait for incoming event
    if (SDL_WaitEvent(&event)) {
      switch (event.type) {
	
        // QUIT (button clicked :)
      case SDL_QUIT: {
        // close button clicked
        fprintf(stderr,"Quit button clicked.");
        run = false;
        break;
      }	

        // Any key down
      case SDL_KEYDOWN: {
        // Switch based on the key that was pressed
        switch (event.key.keysym.sym) {
				case SDLK_ESCAPE:
          fprintf(stderr,"SDL returned ESCAPE event.");
          break;
				
				case SDLK_a:
				case SDLK_b:
				case SDLK_c:
				case SDLK_d:
				case SDLK_e:
          // Trivial cases for some selected types :)
          fprintf(stderr,"SDL returned KEYDOWN event for '%s'\n" , SDL_GetKeyName(event.key.keysym.sym));
          break;
				case SDLK_UP:
          lSpeed = maxSpeed;
          rSpeed = maxSpeed;
          break;
				case SDLK_DOWN:
          lSpeed = -maxSpeed;
          rSpeed = -maxSpeed;
          break;
				case SDLK_LEFT:
          lSpeed = 0;
          rSpeed = maxSpeed;
          break;
				case SDLK_RIGHT:
          lSpeed = maxSpeed;
          rSpeed = 0;
          break;	
				
				
				default:
          // Default, same print =P
						
          fprintf(stderr,"SDL returned KEYDOWN event for '%s'\n" , SDL_GetKeyName(event.key.keysym.sym));
          break;
        }
      }break;
	
      case SDL_KEYUP: {
				fprintf(stderr,"SDL returned KEYUP event for '%s'\n" , SDL_GetKeyName(event.key.keysym.sym));
				//case SDLK_UP:case SDLK_DOWN:case SDLK_LEFT:case SDLK_RIGHT:
        lSpeed = 0;
        rSpeed = 0;
        break;	
      }
	
      default:
        break;
	
      case SDL_JOYBUTTONDOWN:
        switch(event.jbutton.button){ //This is now for the Msoft gamepad
        case 0: //A 
          speedDir=-1;
          autopilot = false;
								
          break;
        case 1: //B
					maxSpeed -=0.1;
					if(maxSpeed<=0.1) maxSpeed = 0.1;
					if(lSpeed > 0) lSpeed=maxSpeed;
					if(rSpeed > 0) rSpeed=maxSpeed;
					autopilot = false;
          break;
        case 2: //X
					maxSpeed +=0.1;
					if(maxSpeed>=0.5) maxSpeed = 0.5;
					if(lSpeed > 0) lSpeed=maxSpeed;
					if(rSpeed > 0) rSpeed=maxSpeed;
					autopilot = false;
          break;
        case 3: //Y
					autopilot = true;
          break;
        case 4: //Left
          lSpeed=maxSpeed;
          autopilot = false;
          break;
        case 5: //Right
          rSpeed=maxSpeed;
          autopilot = false;
          break;
        }
        //fprintf(stderr,"JOY BUTTON DOWN (r=%.2f, l=%.2f) \n",rSpeed,lSpeed);
        break;
      case SDL_JOYBUTTONUP:
        switch(event.jbutton.button){ //This is now for the Msoft gamepad
				case 0: //A -reverse
					speedDir=1;
					autopilot = false; 
          break;
				case 1: //B
          break;
				case 2: //X
          break;
				case 3: //Y
          break;
				case 4: //Left
					if(!autopilot)lSpeed=0;
          break;
				case 5: //Right
					if(!autopilot)rSpeed=0;
          break;
        }
        //fprintf(stderr,"JOY BUTTON UP (r=%.2f, l=%.2f) \n",rSpeed,lSpeed);
        break;
      }
    }
		fprintf(stderr,"Speed Now = (%.2f, %.2f) V=%.2f W=%.2f\n",speedDir*lSpeed,speedDir*rSpeed,
						(lSpeed+rSpeed)/2.0, (lSpeed-rSpeed)/0.2);
		
		
		v = speedDir*(lSpeed+rSpeed)/2.0;
		w = speedDir*(lSpeed-rSpeed)/0.2;

		///Scale down to ease the control
		if(fabs(w) > 0){
      v *= .5;
      w *= .5; 
		}
		
		setSpeedCommanded = true;
  }///WHILE

  fprintf(stderr,"Exit.");
  ownSleep_ms(1000);
  return 0;
}
//*****************************************************************************
//*****************************************************************************
